#include "vins_so/utility/CameraPoseVisualization.h"

const Eigen::Vector3d CameraPoseVisualization::imlt = Eigen::Vector3d( -1.0, -0.5, 1.0 );
const Eigen::Vector3d CameraPoseVisualization::imrt = Eigen::Vector3d( 1.0, -0.5, 1.0 );
const Eigen::Vector3d CameraPoseVisualization::imlb = Eigen::Vector3d( -1.0, 0.5, 1.0 );
const Eigen::Vector3d CameraPoseVisualization::imrb = Eigen::Vector3d( 1.0, 0.5, 1.0 );
const Eigen::Vector3d CameraPoseVisualization::lt0  = Eigen::Vector3d( -0.7, -0.5, 1.0 );
const Eigen::Vector3d CameraPoseVisualization::lt1  = Eigen::Vector3d( -0.7, -0.2, 1.0 );
const Eigen::Vector3d CameraPoseVisualization::lt2  = Eigen::Vector3d( -1.0, -0.2, 1.0 );
const Eigen::Vector3d CameraPoseVisualization::oc   = Eigen::Vector3d( 0.0, 0.0, 0.0 );

void
Eigen2Point( const Eigen::Vector3d& v, geometry_msgs::Point& p )
{
    p.x = v.x( );
    p.y = v.y( );
    p.z = v.z( );
}

CameraPoseVisualization::CameraPoseVisualization( float r, float g, float b, float a )
: m_marker_ns( "CameraPoseVisualization" )
, m_scale( 0.2 )
, m_line_width( 0.01 )
{
    m_image_boundary_color.r           = r;
    m_image_boundary_color.g           = g;
    m_image_boundary_color.b           = b;
    m_image_boundary_color.a           = a;
    m_optical_center_connector_color.r = r;
    m_optical_center_connector_color.g = g;
    m_optical_center_connector_color.b = b;
    m_optical_center_connector_color.a = a;
}

void
CameraPoseVisualization::setImageBoundaryColor( float r, float g, float b, float a )
{
    m_image_boundary_color.r = r;
    m_image_boundary_color.g = g;
    m_image_boundary_color.b = b;
    m_image_boundary_color.a = a;
}

void
CameraPoseVisualization::setOpticalCenterConnectorColor( float r, float g, float b, float a )
{
    m_optical_center_connector_color.r = r;
    m_optical_center_connector_color.g = g;
    m_optical_center_connector_color.b = b;
    m_optical_center_connector_color.a = a;
}

void
CameraPoseVisualization::setScale( double s )
{
    m_scale = s;
}
void
CameraPoseVisualization::setLineWidth( double width )
{
    m_line_width = width;
}
void
CameraPoseVisualization::add_edge( const Eigen::Vector3d& p0, const Eigen::Vector3d& p1 )
{
    visualization_msgs::Marker marker;

    marker.ns      = m_marker_ns;
    marker.id      = m_markers.size( ) + 1;
    marker.type    = visualization_msgs::Marker::LINE_LIST;
    marker.action  = visualization_msgs::Marker::ADD;
    marker.scale.x = 0.005;

    marker.color.g = 1.0f;
    marker.color.a = 1.0;

    geometry_msgs::Point point0, point1;

    Eigen2Point( p0, point0 );
    Eigen2Point( p1, point1 );

    marker.points.push_back( point0 );
    marker.points.push_back( point1 );

    m_markers.push_back( marker );
}

void
CameraPoseVisualization::add_loopedge( const Eigen::Vector3d& p0, const Eigen::Vector3d& p1 )
{
    visualization_msgs::Marker marker;

    marker.ns      = m_marker_ns;
    marker.id      = m_markers.size( ) + 1;
    marker.type    = visualization_msgs::Marker::LINE_LIST;
    marker.action  = visualization_msgs::Marker::ADD;
    marker.scale.x = 0.02;
    // marker.scale.x = 0.3;

    marker.color.r = 1.0f;
    marker.color.b = 1.0f;
    marker.color.a = 1.0;

    geometry_msgs::Point point0, point1;

    Eigen2Point( p0, point0 );
    Eigen2Point( p1, point1 );

    marker.points.push_back( point0 );
    marker.points.push_back( point1 );

    m_markers.push_back( marker );
}

void
CameraPoseVisualization::add_pose( const Eigen::Vector3d& p, const Eigen::Quaterniond& q )
{
    visualization_msgs::Marker marker;

    marker.ns      = m_marker_ns;
    marker.id      = m_markers.size( ) + 1;
    marker.type    = visualization_msgs::Marker::LINE_STRIP;
    marker.action  = visualization_msgs::Marker::ADD;
    marker.scale.x = m_line_width;

    marker.pose.position.x    = 0.0;
    marker.pose.position.y    = 0.0;
    marker.pose.position.z    = 0.0;
    marker.pose.orientation.w = 1.0;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;

    geometry_msgs::Point pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, pt_lt2;

    Eigen2Point( q * ( m_scale * imlt ) + p, pt_lt );
    Eigen2Point( q * ( m_scale * imlb ) + p, pt_lb );
    Eigen2Point( q * ( m_scale * imrt ) + p, pt_rt );
    Eigen2Point( q * ( m_scale * imrb ) + p, pt_rb );
    Eigen2Point( q * ( m_scale * lt0 ) + p, pt_lt0 );
    Eigen2Point( q * ( m_scale * lt1 ) + p, pt_lt1 );
    Eigen2Point( q * ( m_scale * lt2 ) + p, pt_lt2 );
    Eigen2Point( q * ( m_scale * oc ) + p, pt_oc );

    // image boundaries
    marker.points.push_back( pt_lt );
    marker.points.push_back( pt_lb );
    marker.colors.push_back( m_image_boundary_color );
    marker.colors.push_back( m_image_boundary_color );

    marker.points.push_back( pt_lb );
    marker.points.push_back( pt_rb );
    marker.colors.push_back( m_image_boundary_color );
    marker.colors.push_back( m_image_boundary_color );

    marker.points.push_back( pt_rb );
    marker.points.push_back( pt_rt );
    marker.colors.push_back( m_image_boundary_color );
    marker.colors.push_back( m_image_boundary_color );

    marker.points.push_back( pt_rt );
    marker.points.push_back( pt_lt );
    marker.colors.push_back( m_image_boundary_color );
    marker.colors.push_back( m_image_boundary_color );

    // top-left indicator
    marker.points.push_back( pt_lt0 );
    marker.points.push_back( pt_lt1 );
    marker.colors.push_back( m_image_boundary_color );
    marker.colors.push_back( m_image_boundary_color );

    marker.points.push_back( pt_lt1 );
    marker.points.push_back( pt_lt2 );
    marker.colors.push_back( m_image_boundary_color );
    marker.colors.push_back( m_image_boundary_color );

    // optical center connector
    marker.points.push_back( pt_lt );
    marker.points.push_back( pt_oc );
    marker.colors.push_back( m_optical_center_connector_color );
    marker.colors.push_back( m_optical_center_connector_color );

    marker.points.push_back( pt_lb );
    marker.points.push_back( pt_oc );
    marker.colors.push_back( m_optical_center_connector_color );
    marker.colors.push_back( m_optical_center_connector_color );

    marker.points.push_back( pt_rt );
    marker.points.push_back( pt_oc );
    marker.colors.push_back( m_optical_center_connector_color );
    marker.colors.push_back( m_optical_center_connector_color );

    marker.points.push_back( pt_rb );
    marker.points.push_back( pt_oc );
    marker.colors.push_back( m_optical_center_connector_color );
    marker.colors.push_back( m_optical_center_connector_color );

    m_markers.push_back( marker );
}

void
CameraPoseVisualization::reset( )
{
    m_markers.clear( );
}

void
CameraPoseVisualization::publish_by( ros::Publisher& pub, const std_msgs::Header& header )
{
    visualization_msgs::MarkerArray markerArray_msg;

    for ( auto& marker : m_markers )
    {
        marker.header = header;
        markerArray_msg.markers.push_back( marker );
    }

    pub.publish( markerArray_msg );
}
